#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

void poseReceived(nav_msgs::Odometry odom) {
	static tf::TransformBroadcaster odom_broadcaster;
	geometry_msgs::TransformStamped odom_trans;

	odom_trans.header.stamp = ros::Time::now();
	odom_trans.header.frame_id = "/odom";
	odom_trans.child_frame_id = "/base_link";

	odom_trans.transform.translation.x = odom.pose.pose.position.x;
	odom_trans.transform.translation.y = odom.pose.pose.position.y;
	odom_trans.transform.translation.z = odom.pose.pose.position.z;

	odom_trans.transform.rotation.x = odom.pose.pose.orientation.x;
	odom_trans.transform.rotation.y = odom.pose.pose.orientation.y;
	odom_trans.transform.rotation.z = odom.pose.pose.orientation.z;
	odom_trans.transform.rotation.w = odom.pose.pose.orientation.w;

       	odom_broadcaster.sendTransform(odom_trans);
}

int main(int argc, char** argv){
	ros::init(argc, argv, "odometry_publisher");

	ros::NodeHandle n;
	ros::Subscriber odom_sub = n.subscribe("/base_link", 20, poseReceived);
	ros::spin();
}
